﻿#ifndef ROBOTARM_H
#define ROBOTARM_H

#include <QtNetwork/QTcpSocket>
#include <QJsonObject>
#include <QJsonDocument>
#include <QJsonArray>
#include <QJsonValue>
#include <QDateTime>
#include <QByteArray>
#include <QTimer>
#include <QCoreApplication>
#include "robotarmmonitor.h"
#include <QThread>
#include <qmath.h>
#include <QMessageBox>

#include "robot_type.h"

#include "Calibration/tf/LinearMath/Transform.h"

#pragma execution_character_set("utf-8")


class RobotArm : public QObject
{

    Q_OBJECT

public:
    RobotArm(QObject *parent = Q_NULLPTR);

    //连接机械臂服务器 (控制接口)
    bool connectToHost(std::string host, int port);
    //连接机械臂服务器(监控接口)
    bool connectToMonitorHost(std::string host, int port);

    bool isConnected();

    //断开机械臂服务器
    bool disconnectToHost();
    //机械臂初始化
    bool robotArmInit();
    //机器人停几秒
    void robot_sleep(unsigned int msec);

    void set_arm_speed(int value);
    void set_arm_acc(int value);
    void set_arm_dec(int value);

    void set_robot_monitor(RobotArmMonitor *monitor, QString host);

    //
    RobotPoseJoint getRobotPoseJoint();
    RobotPoseXYZ   getRobotPoseXYZ();

    bool getRobotPoseXYZ(QString station_id, QString pose_name , RobotPoseXYZ& pose);

    //获取机械臂法兰盘姿态
    bool clearAlarm();
    bool syncMotorStatus();
    bool set_servo_status(bool state);
    bool calibrate_encoder_zero_position();

    //void getRobotBaseFlangePose();

    //移动到目标点位
    bool move_to_common_pose(QString pose_name,
                             MoveType type,
                             double speed,
                             int acceleration,
                             int deceleration,
                             BlockMotion blockmotion);
    
    bool move_to_station_pose(QString station_id,
                              QString pose_name,
                              MoveType type,
                              double speed,
                              int acceleration,
                              int deceleration,
                              BlockMotion blockmotion);
    //
    bool move_to_pose(RobotPoseJoint pose,
                      MoveType move_type,
                      double speed,
                      int acceleration,
                      int deceleration,
                      BlockMotion blockmotion);

    bool move_to_pose(RobotPoseXYZ pose,
                      MoveType move_type,
                      double speed,
                      int acceleration,
                      int deceleration,
                      BlockMotion blockmotion);

    //
    bool move_to_pose(MoveType type, QString pose_name);


    bool move_by_joint(RobotPoseJoint& pose,
                       double speed,
                       int acceleration,
                       int deceleration,
                       BlockMotion blockmotion);

    bool move_by_line(RobotPoseJoint& pose,
                      double speed,
                      int acceleration,
                      int deceleration,
                      BlockMotion blockmotion);

    bool stop_movement();

    //暂停
    bool pause_movement();
    //恢复运动
    bool run_movement();

    //逆解，XYZ坐标转JOINT坐标
    bool getIK(RobotPoseXYZ& pose_xyz, RobotPoseJoint& pose_joint);

    //正解， JOINT坐标 转 XYZ坐标
    bool getFK(RobotPoseJoint pose_joint, RobotPoseXYZ& pose_xyz);

    //执行机械臂指令; return true or false;
    bool execSpecifiedCommand(QJsonObject& _JsonCommand);
    //执行movebypath; return 路点个数
    bool execMoveByPathCommand(QJsonObject& _JsonCommand);

    //废弃
    QString getRobotState();
    //废弃
    QString robotStateParse(QByteArray &_robotStateResponse);

    //
    bool isContainCommonPose(QString name);
    bool isContainStationPose(QString station_id, QString name);
    
    bool add_common_pose(QString name, RobotPoseJoint pose);
    bool update_common_pose(QString name, RobotPoseJoint pose);
    bool remove_common_pose(QString name);

    bool add_station_pose(QString station_id, QString name, RobotPoseXYZ pose);
    bool update_station_pose(QString station_id, QString name, RobotPoseXYZ pose);
    bool remove_station_pose(QString station_id, QString name);

    std::map<QString, RobotPoseJoint> getRobotCommonPose();
    std::map<QString,std::map<QString, RobotPoseXYZ>> getRobotStationPose();

    bool getRobotStationPose(QString station_id, QString name, RobotPoseXYZ& pose);
    bool getRobotCommonPose(QString name, RobotPoseJoint& pose);

    bool set_DO(int id, int value);
    bool get_DI(int id, int& data);
    bool set_acceleration_value(int value);
    bool set_deceleration_value(int value);

    bool copy_station_data(QString copy_station_id,
                           QString target_station_id);

    //movement
    void moveToXMinus();
    void moveToXPlus();
    void moveToYMinus();
    void moveToYPlus();
    void moveToZMinus();
    void moveToZPlus();
    void moveToRollMinus();
    void moveToRollPlus();
    void moveToPitchMinus();
    void moveToPitchPlus();
    void moveToYawMinus();
    void moveToYawPlus();
    void moveToJoint1Minus();
    void moveToJoint1Plus();
    void moveToJoint2Minus();
    void moveToJoint2Plus();
    void moveToJoint3Minus();
    void moveToJoint3Plus();
    void moveToJoint4Minus();
    void moveToJoint4Plus();
    void moveToJoint5Minus();
    void moveToJoint5Plus();
    void moveToJoint6Minus();
    void moveToJoint6Plus();

    //点位距离检测

    bool is_current_pose(QString pose_name, float distance);

    //当前机械臂模式
    bool is_remote_mode();

    //增加通用路点信息
    bool add_common_waypoint(QString pose_name,
                             MoveType type,
                             int speed,
                             int circular_radius);
    //增加机台路点信息
    bool add_station_waypoint(QString station_id,
                             QString pose_name,
                             MoveType type,
                             int speed,
                             int circular_radius);

    bool add_xyz_waypoint(RobotPoseXYZ pose,
                             MoveType type,
                             int speed,
                             int circular_radius);

    bool add_joint_waypoint(RobotPoseJoint pose,
                             MoveType type,
                             int speed,
                             int circular_radius);

    //清除路点信息
    bool clear_waypoints();
    //执行轨迹运动
    bool move_by_path(BlockMotion blockmotion);

    //
    void set_agv_emergency_state(bool state);

signals:
    void sig_start_monitor();


private:
     QTcpSocket* control_socket_;

     RobotArmMonitor *robot_arm_monitor_;

     //QThread* robot_monitor_thread_;

     QJsonArray velocity;
     QJsonObject params;
     QJsonDocument robotCommand;
     QByteArray command;

     QString robotState = "-1";

     QTimer* timer_get_robot_state_;

     std::map<QString, RobotPoseJoint> robot_common_poses_;

     std::map<QString, std::map<QString,RobotPoseXYZ>> robot_station_poses_;

     double GeneralSpeed = 15;
     int GeneralACC = 15;
     int GeneralDEC = 15;

     bool agv_emergency_stop = false;

};

#endif // ROBOTARM_H
